#include "heightfield.h"

namespace mGameEngine
{

Heightfield::Heightfield() :
    Shape(SHAPE_HEIGHTFIELD),
    _size(), _position(),
    _columns(), _rows(), 
    _heights(NULL)
{
}

Heightfield::Heightfield(const Vector3 &size, const Vector3 &position,
                         uint columns, uint rows, 
                         float *heights) :
    Shape(SHAPE_HEIGHTFIELD),
    _size(size), _position(position),
    _columns(), _rows(),
    _heights(NULL)
{
    setHeights(columns, rows, heights);
}

Heightfield::~Heightfield()
{
    if(_heights)
    {
        free(_heights);
    }
}

void Heightfield::setHeights(uint columns, uint rows, float *heights)
{
    // no data specified
    if(!columns || !rows || !heights)
    {
        return;
    }

    _columns = columns;
    _rows = rows;

    uint sz = sizeof(float) * _columns * _rows ;
    if(!(_heights = (float *)malloc(sz)))
    {
        return;
    }
    memcpy(_heights, heights, sz);
}

float Heightfield::getHeightAt(const Vector2 &p) const
{
    // no heights specified or position outside heightfield
    if(!_heights ||
        p.x < _position.x || p.x > _position.x + _size.x ||
        p.y < _position.y || p.y > _position.y + _size.y)
    {
        return 0.0f;
    }

    // get heightfield value
    float x = clamp((p.x - _position.x) / _size.x, 0.0f, 1.0f) * (_columns-1);
    float y = clamp((p.y - _position.y) / _size.y, 0.0f, 1.0f) * (_rows-1);
    
    uint x0 = uint(floor(x)), x1 = uint(ceil(x));
    uint y0 = uint(floor(y)), y1 = uint(ceil(y));

    float h00 = _heights[x0 + y0 * _columns];
    float h01 = _heights[x0 + y1 * _columns];
    float h10 = _heights[x1 + y0 * _columns];
    float h11 = _heights[x1 + y1 * _columns];

    float f;
    float lerp1 = modf(x, &f);
    float lerp2 = modf(y, &f);
    float h = bilerp(h00, h10, h01, h11, lerp1, lerp2) * _size.z + _position.z;
    return h;
}

Vector3 Heightfield::getNormalAt(const Vector2 &p) const
{
    // no heights specified or position outside heightfield
    if(!_heights ||
        p.x < _position.x || p.x > _position.x + _size.x ||
        p.y < _position.y || p.y > _position.y + _size.y)
    {
        return Vector3(0.0f, 0.0f, 1.0f);
    }

    // calculate normal
    float h = getHeightAt(p);
    Vector3 dx(0.1f, 0.0f, getHeightAt(p + Vector2(0.1f, 0.0f)) - h);
    Vector3 dy(0.0f, 0.1f, getHeightAt(p + Vector2(0.0f, 0.1f)) - h);
    Vector3 n = cross(dx.normalize(), dy.normalize());
    return n;
}

float Heightfield::getSlopeAt(const Vector2 &p) const
{
    // no heights specified or position outside heightfield
    if(!_heights ||
        p.x < _position.x || p.x > _position.x + _size.x ||
        p.y < _position.y || p.y > _position.y + _size.y)
    {
        return 0.0f;
    }

    // calculate slope
    Vector3 n = getNormalAt(p);
    float slope = acosf(dot(n, Vector3(0.0f, 0.0f, 1.0f)));
    return slope;
}

}
